
/************************************************************************************************* 

	Name: MOTORS_C
	Autor: JA&AP
	
	Contains: 
				File containing all the functions related to the Motors.
				
				
**************************************************************************************************/
#include <stdint.h>
#include <stdlib.h>
#include "nxbot.h"
#include "motors.h"#include <avr/pgmspace.h>

#define OFFSET_DEAD_ZONE 20
//LUT for linearize the response of the motor
//See Documentation for more details.
unsigned char motorLinear[21] PROGMEM = {0, 1, 3, 4, 6, 7, 9, 11, 13, 15, 17, 20, 23, 26, 30, 35, 40, 47, 58, 75, 100};


static int motors_getActualRVel(unsigned int sampleFreq);
static int motors_getActualLVel(unsigned int sampleFreq);

void motors_getActualVels(velocities *vels, unsigned int sampleFreq)
{
	vels->velRightAct = motors_getActualRVel(sampleFreq);
	vels->velLeftAct = motors_getActualLVel(sampleFreq);
	
}
// para cualcular la velocidad se debe enviar la variacion de tiempo con la que se esta calculando. 
//Luego de leida la vel, se resetea el encoder. 
//Se utiliza la constante ENC_CONV para establecer el factor de conversion de pulsos/tiempo a metros/segundo
static int motors_getActualRVel(unsigned int sampleFreq){

	static unsigned int lastEnc;
	unsigned int enc;
	unsigned int temp;
	
	//get encoders count
	enc=encoders_getStateRight();

	if(enc>=lastEnc) 
		temp=enc-lastEnc;
	else 
		temp=enc+(UINT16_MAX-lastEnc);
		
	lastEnc=enc;

	temp = temp * VEL_FACT/sampleFreq;
	
	if (temp>100)
		temp = 100;
		
	#if INVERT_PWM==0
		if(bit_is_clear(PORTG,DIR_RIGHT)) return -temp;
		else return temp;
	#else
		if(bit_is_set(PORTG,DIR_RIGHT)) return -temp;
		else return temp;
	#endif
	
}

// para cualcular la velocidad se debe enviar la variacion de tiempo con la que se esta calculando. 
//Luego de leida la vel, se resetea el encoder. 
//Se utiliza la constante ENC_CONV para establecer el factor de conversion de pulsos/tiempo a metros/segundo
static int motors_getActualLVel(unsigned int sampleFreq){

	static unsigned int lastEnc=0;
	unsigned int enc=0;
	unsigned int temp;

	enc=encoders_getStateLeft();

	if(enc>=lastEnc) 
		temp=enc-lastEnc;
	else 
		temp=enc+(UINT16_MAX-lastEnc);
	
	lastEnc = enc;
	
	temp = temp * VEL_FACT/sampleFreq;
		
	
	if (temp>100)
		temp = 100;
		
	#if INVERT_PWM==0
		if(bit_is_set(PORTG,DIR_LEFT)) return -temp;
		else return temp;
	#else
		if(bit_is_clear(PORTG,DIR_LEFT)) return -temp;
		else return temp;
	#endif

}

//SE configuran pwm y encoders y se arrancan encoeders
void motors_init(unsigned char freqPWM){
	configPwm(freqPWM);
	encoders_init();
	encoders_start();
}



//establecer velocidad de los motores (esta rutina se debe utilizar en CONTROLLER, aun no ha sido utilizada) Se usaran las constantes MOTOR_RIGHT y MOTOR_LEFT para definir los motores en esta funcion, segundo parametro
//vel es un valor entre 0 y 100
void motors_setVel(int vel,unsigned char motor){

	char pwm;
	char velLin;
	
	if(vel>100)
		vel = 100;
	if(vel<-100)
		vel = -100;
		
	//first, linealize velocity
	if(vel<0)
		velLin = pgm_read_byte(&motorLinear[-vel/5]);
	else
		velLin = pgm_read_byte(&motorLinear[vel/5]);
	
	if(vel<0)
		velLin *= -1;
	//restrict velocity value to MAX
	if(velLin>100)
		velLin = MAX_VEL;
	else if(velLin <-100)
		velLin = -MAX_VEL;
	
	//if velocity is cero, then pwm should be off (0)
	if(velLin>0)
		pwm = velLin + OFFSET_DEAD_ZONE;
	else if(velLin<0)
		pwm = velLin - OFFSET_DEAD_ZONE;
	else
		pwm = 0;

	
	//if velocity is MAX, then pwm to MAX
	if(vel == 100)
		pwm = MAX_PWM;
	else if (vel == -100)
		pwm = -MAX_PWM;
	
	
	if(motor == MOTOR_RIGHT)
		setPwmRight(pwm);
	else
		setPwmLeft(pwm);
}

